#include "DefendingState.h"
#include "TargetNearestTankState.h"
#include "World.h"
#include "PDController.h"
#include "AttractiveField.h"
#include "RepulseField.h"
#include "KalmanTank.h"

bool DefendingState::initialized = false;
vector<Vector2> DefendingState::defense_positions;
map<int, int> DefendingState::assigned_positions;

DefendingState::DefendingState(void)
{
	has_pos = false;
	if (!initialized) {
		init();
	}
}


DefendingState::~DefendingState(void)
{
}

void DefendingState::init() {
	initialized = true;
	World* world = World::instance();
	KalmanTank* ktank = &world->enemies()[0];
	Vector2 center_pos = world->getFlagPos(world->getTeam());
	if (center_pos.x < 0) {
		center_pos.x += 15;
	}
	else{
		center_pos.x -= 15;
	}
	AttractiveField afield(ktank->GetCurrentEstimate());
	Vector2 trajectory = afield.get_trajectory(center_pos);
	Vector2 defense_line = trajectory.orthogonal();
	//Vector2 defense_line(0, 1);
	defense_positions.push_back(center_pos);

	int step = 8;

	for (unsigned int i = 1; i < world->tanks().size(); i += 2) {
		defense_positions.push_back(center_pos + defense_line * (i * step));
		defense_positions.push_back(center_pos + defense_line * (i * -step));
	}

	for (unsigned int i = 0; i < world->tanks().size(); i++) {
		assigned_positions[world->tanks().at(i).index] = -1;
	}

	for (unsigned int i = 0; i < defense_positions.size(); i++) {
		double min_dist = 999999;
		tank_t* assigned_tank = 0;
		for (unsigned int j = 0; j < world->tanks().size(); j++) {
			tank_t* tank = &world->tanks().at(j);
			Vector2 tank_pos = Vector2(tank->pos[0], tank->pos[1]);
			double dist = (tank_pos - defense_positions[i]).magnitude();
			if (dist < min_dist && assigned_positions[tank->index] == -1) {
				min_dist = dist;
				assigned_tank = tank;
			}
		}
		if (assigned_tank) {
			assigned_positions[assigned_tank->index] = i;
		}
	}
	iterationsStuck = 0;
}

Command DefendingState::GetCommand(Agent* agent) {
	World* world = World::instance();
	/*if (!has_pos) {
		int index = assigned_positions[agent->getTank()->index];
		if (index >= 0) {
			pos = defense_positions[index];
		}
		else {
			pos = Vector2(agent->getTank()->pos[0], agent->getTank()->pos[1]);
		}
		has_pos = true;
	}*/

	KalmanTank* ktank = GetAvailableTankInRange(agent);

	Vector2 tank_pos = Vector2(agent->getTank()->pos[0], agent->getTank()->pos[1]);
	if((tank_pos - previousPos).magnitude() < .1)
		iterationsStuck ++;
	else
		iterationsStuck = 0;

	AttractiveField afield(pos);
	Vector2 trajectory = afield.get_trajectory(tank_pos);

	/*for (unsigned int i = 0; i < world->tanks().size(); i++) {
		tank_t* tank = &world->tanks().at(i);
		if (tank == agent->getTank())
			continue;
		RepulseField rfield(Vector2(tank->pos[0], tank->pos[1]), 50);
		trajectory += rfield.get_trajectory(tank_pos);
	}*/

	Vector2 baseMiddle = World::instance()->getBasePos(World::instance()->getTeam());
	//RepulseField rfield(baseMiddle, 30);
	trajectory = (tank_pos - baseMiddle);//rfield.get_trajectory(tank_pos);
	if(trajectory.magnitude() < .01)
		trajectory.x = 1.0f;
	// If there is an enemy tank in range or we have settled into our defense spot
	//if (ktank || trajectory.magnitude() < .1) { 
	if(trajectory.magnitude() > 50 || iterationsStuck > 10){
		agent->setState(new TargetNearestTankState(false), false);
		return agent->GetCommand();
	}
	
	trajectory = trajectory * 5;
	Command cmd = PDController::instance()->GetCommand(agent->getTank(), trajectory);
	//cmd.angvel(cmd.angvel() * .2);
	return cmd;
}

KalmanTank* DefendingState::GetAvailableTankInRange(Agent* agent) {
	World* world = World::instance();
	KalmanTank* retVal = 0;

	for (unsigned int i = 0; i < world->enemies().size(); i++) {
		tank_t* tank = agent->getTank();
		KalmanTank* ktank = &world->enemies().at(i);
		otank_t* otank = ktank->GetStruct();

		if(otank->status.compare("dead")==0)
			continue;

		Vector2 tank_pos(tank->pos[0], tank->pos[1]);
		Vector2 enemy_pos(otank->pos[0], otank->pos[1]);

		double mag = (tank_pos - enemy_pos).magnitude();
		if (mag <= world->getShotRange()) {
			retVal = ktank;
			break;
		}
	}

	return retVal;
}